#include <unionsys_core.hpp>
#include <iostream>  


class UnionsysEKF{  
public:  
    UnionsysEKF();
    void predict();
    void update(const Eigen::VectorXd& z);

    Eigen::VectorXd getState() const {  
        return x_;  
    }  
    Eigen::VectorXd getK_() const {  
        return K_;  
    }    
    void set_dt(float t_set){
        dt_ = t_set;
        F_ << 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_, 0, 0,  
         0, 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_, 0,
         0, 0, 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_,
         0, 0, 0, 1, 0, 0, dt_, 0, 0,
         0, 0, 0, 0, 1, 0, 0, dt_, 0,
         0, 0, 0, 0, 0, 1, 0, 0, dt_,
         0, 0, 0, 0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 0, 0, 0, 1;
         
    }

private:  
    Eigen::VectorXd x_;  
    Eigen::MatrixXd P_; 
    Eigen::MatrixXd F_;  
    Eigen::MatrixXd H_;   
    Eigen::MatrixXd R_;   
    Eigen::MatrixXd Q_;  
    Eigen::MatrixXd K_; 
    float dt_;
};

